module necrophilia.skeleton.Bone;
import maths.CoordSys;
import maths.Vec;
import necrophilia.skeleton.DataNodes;
import utils.Memory;
import maths.Misc;
import maths.Quat;


struct Bone(Anim){
	
	protected Bone!(Anim) *[] _children;
	protected Bone!(Anim) * _parent;
	protected int _index;
	package CoordDump coordDump;
	CoordSys coordSys;
	
	Bone!(Anim) clone(){
		Bone!(Anim) twin;
		twin._children=this._children.dup;
		twin._parent=this._parent;
		twin._index=this._index;
		twin.coordSys=this.coordSys;
		return twin;
	}
	
	/+void nest(CoordSys parent){
		coordSys= coordSys in parent;
		foreach(child; _children) child.nest(coordSys);
	}
	
	void moveTo(CoordSys ns){
		coordSys=ns;
	}+/
	
	Bone!(Anim) *[] children() {
		return _children;
	}
	
	void reset(){
		//writefln("reset!");
		coordDump.reset;
		//writefln("after reset len ", coordDump.getLen);
	}
	
	
	void setIndex(int i){
		_index=i;
	}
	
	int index(){
		return _index;
	}
	
	void setParent(Bone!(Anim) *p){
		_parent=p;
	}
	
	void addChild(Bone!(Anim) *c){
		_children.append(c);
	}
	
	void registerToAnimation(Anim animation){
		//writefln("bone index: ",_index);
		animation.setDumpMethod(&coordDump.dumpCoord, _index);
		//writefln(coordDump.getSize);
		coordDump.setSize(coordDump.getSize+1);
		//writefln("set size to: ", coordDump.getSize);
	}
	
	void buildReferences(inout Bone!(Anim)[] bones){
		if(_parent !is null){
			_parent=&bones[_parent.index];
		}
		foreach(inout child; _children){
			child=&bones[child.index];
		}
		foreach(inout child; _children){
			child.buildReferences(bones);
		}
	}
	
	CoordSys oldCoords=CoordSys.identity;
	bool first=true;
	float transAdaptibility=0.1;
	float rotAdaptibility=0.1;
	
	void setAdaptibilities(float t,float r){
		 transAdaptibility=t;
		 rotAdaptibility=r;
	}
	
	void moveTo(CoordSys ns){
		coordSys=ns;
	}
	
	void nest(CoordSys parent){
		if(first) {
			oldCoords=coordSys;
			first=false;
		}
		
		float adaptibility=transAdaptibility;
		float time= (vec3.convert(oldCoords.origin)-vec3.convert(coordSys.origin)).len;
		if(time==0) time=1; else time = adaptibility/time;
		if(time>1)time=1;
		linearInterp!(vec3fi)(time,oldCoords.origin,coordSys.origin,coordSys.origin);
		
		float radaptibility= rotAdaptibility;
		time= (oldCoords.rotation.xform(vec3.unitX)-coordSys.rotation.xform(vec3.unitX)).len;
		time+= (oldCoords.rotation.xform(vec3.unitY)-coordSys.rotation.xform(vec3.unitY)).len;
		time+= (oldCoords.rotation.xform(vec3.unitZ)-coordSys.rotation.xform(vec3.unitZ)).len;
		if(time==0) time=1; else time = radaptibility/time;
		if(time>1)time=1;
		coordSys.rotation=quat.slerp(oldCoords.rotation,coordSys.rotation,time);
		
		
		oldCoords=coordSys;
		
		coordSys=coordSys in parent;	
		
		foreach(child; _children) child.nest(coordSys);
		
		
	} 

}





import std.stdio;